﻿#include "RobotHandTest.h"
#include "common_log.h"

constexpr int g_dataLength = 64;
constexpr int g_stateLength = 133;

RobotHandTest::RobotHandTest(QObject* parent)
	: RobotHand(parent)
{
	m_brand = "KUKA";
	log_info("-----------------Kuka Robot has been create-------------");
	m_cmd = new TCPClient(nullptr);
	connect(m_cmd, &TCPClient::TcpDisconnected, [&]() {
		log_error("命令端口断开连接");
		emit ConnectionStateChanged(2);
		});

	m_status = new StatusThread();
	connect(m_status, &StatusThread::statusUpdated, this, &RobotHandTest::OnStatusDataReady, Qt::DirectConnection);
	connect(m_status, &StatusThread::connectionStatusChanged, this, &RobotHandTest::OnDisConnection);
}

RobotHandTest::~RobotHandTest()
{}

// 初始
int RobotHandTest::Init(const RobotParam& param)
{
	// 参数校验
	if (param.id.isEmpty() || param.ip.isEmpty()) {
		log_error("Kuka robot init param error, id is [{}] or ip is [{}]", param.id, param.ip);
		return -1;
	}
	if (param.statusPort <= 0 || param.cmdPort <= 0) {
		log_error("Kuka robot init prot error, state is [{}] or cmd is [{}]", param.statusPort, param.cmdPort);
		return -1;
	}
	m_info = param;
	log_trace("机械手IP:{}, 命令端口：{} 状态端口：{}", m_info.ip, m_info.cmdPort, m_info.statusPort);
	if (!m_cmd->ConnectToHost(m_info.ip, m_info.cmdPort)) {
		log_error("cmd port connect failed");
		log_error("命令端口连接失败，IP={} port={}", m_info.ip, m_info.cmdPort);
		m_connect += -2;
		return -2;
	}
	if (m_cmd->WaitForConnected(5000)) {
		log_trace("命令端口连接成功");
	} else {
		m_connect = -2;
		log_trace("命令端口连接失败");
		return -2;
	}
	m_stateTimes = 0;
	m_lastData.clear();

	m_status->Init(param.ip, param.statusPort);
	m_status->start();
	return 0;
}

/****************************************************************************
* 功能: 重连接口
* 传参:
* 返回值: 重连是否成功返回0， 状态端口失败返回-1，命令端口失败返回-2, 全部失败返回-3
****************************************************************************/
int RobotHandTest::Reconnecte()
{
	if (!m_cmd->ConnectToHost(m_info.ip, m_info.cmdPort)) {
		log_error("cmd port connect failed");
		log_error("命令端口连接失败，IP={} port={}", m_info.ip, m_info.cmdPort);
		emit ConnectionStateChanged(2);
		return -2;
	}
	if (m_cmd->WaitForConnected(5000)) {
		log_trace("命令端口连接成功");
	} else {
		emit ConnectionStateChanged(2);
		log_trace("命令端口连接失败");
		return -2;
	}
	m_stateTimes = 0;
	m_lastData.clear();

	m_status->Stop();
	m_status->start();
	return 0;
}
int RobotHandTest::GetConnectedState()
{
	return 0;
}
/****************************************************************************
* 功能: 停止接口，机械手停止运动并清理运动轨迹
* 传参:
* 返回值:
****************************************************************************/
bool RobotHandTest::Stop()
{
	log_trace("Robot stop cmd");
	if (SendCmdData(1, "")) {
		return true;
	} else {
		log_error("robot set Stop failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 暂停/恢复接口，机械手暂停，不清理轨迹，机械手恢复后继续运动
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::Pause()
{
	log_trace("robot set pause");
	QByteArray data = ToBin(2, 4);
	if (SendCmdData(2, data)) {
		return true;
	} else {
		log_error("robot set pause failed");
		return false;
	}
}
bool RobotHandTest::Resume()
{
	log_trace("robot set pause");
	QByteArray data = ToBin(1, 4);
	if (SendCmdData(2, data)) {
		return true;
	} else {
		log_error("robot set pause failed");
		return false;
	}
}

/****************************************************************************
 * 功能: 复位机械手，清除机械手程序错误，以及点位信息
 * 传参:
 * 返回值:
 ****************************************************************************/
bool RobotHandTest::Reset()
{
	QMutexLocker locker(&stateMutex);
	m_lastData.clear();
	return true;
}

/****************************************************************************
	* 功能: 设置和获取当前的TCPBase(未启用)
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::SetTcpBase(const QString& info)
{
	return false;
}
QString RobotHandTest::GetTcpBase()
{
	return "";
}

/****************************************************************************
	* 功能: 设置DO
	* 传参: index: DO编号(1-n), value: true=1,false=0;
	* 返回值:
	****************************************************************************/
bool RobotHandTest::SetDo(unsigned int index, bool value)
{
	QByteArray data = ToBin(1, 4);	// index
	data.append(ToBin((int)index, 4));		// val;
	data.append(ToBin(value ? 1 : 0, 4));	// bits;
	log_trace("Set DO index [{}] to [{}]", index, value ? 1 : 0);
	if (SendCmdData(4, data)) {
		return true;
	} else {
		log_error("robot set DO failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 夹具动作/预定义动作
	* 传参: index: 预先定义的动作编号（由机械手内部程序定义）
	* 返回值:
	****************************************************************************/
bool RobotHandTest::SetGrip(unsigned int index, const GripParam& param)
{
	QByteArray data = ToBin((int)index, 4);	// index
	log_trace("Set grip index [{}] to [{}]", index);
	if (SendCmdData(5, data)) {
		return true;
	} else {
		log_error("robot set grip failed");
		return false;
	}
}

/****************************************************************************
	* 功能: 移动指令
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::MoveJL(const MoveJLInfo& move)
{
	log_trace("movejl point id = {}", move.id);

	QByteArray data = ToBin(1, 4);
	data.append(ToBin(move.pointType, 4));	// 点的类型
	data.append(ToBin(move.id, 4));		// 点 id
	data.append(ToBin(move.pos.x, 4));		// 坐标
	data.append(ToBin(move.pos.y, 4));
	data.append(ToBin(move.pos.z, 4));
	data.append(ToBin(move.pos.a, 4));
	data.append(ToBin(move.pos.b, 4));
	data.append(ToBin(move.pos.c, 4));
	data.append(ToBin(move.vel, 4));		// 速度
	data.append(ToBin(move.zone, 4));	// 圆滑
	data.append(ToBin(move.acc, 4));		// 加速度
	data.append(ToBin(move.di, 4));		// 停止di
	if (!SendCmdData(6, data)) {
		log_error(" Send movejl info error,xyz:({}, {}, {}),abc：({}, {}, {})", 
			move.pos.x, move.pos.y, move.pos.z,
			move.pos.a, move.pos.b, move.pos.c);
		return false;
	}

	return true;
}

/****************************************************************************
	* 功能: 特殊指令
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::ExecCmd(const QString& cmd, const QString& para, QString& result)
{
	return false;
}

void RobotHandTest::ParaseRobotStateData()
{
	int index = 0;
	QVector<int> length = { 4 * 6, 4 * 6, 4, 4, 4, 4, 4, 4, 4, 4 };
	QByteArray data = m_lastData;
	QByteArrayList infos = data.split(',');

	if (infos.size() < 18) {
		int fault = 1;
		QString err = "状态数据异常";
		emit RobotHandFaultCode(fault, err);
		return;
	}

	ParseTargets(infos);
	ParseJoints(infos);
	ParsePointId(infos);
	ParseMoving(infos);
	ParseToolId(infos);
	ParseFaultCode(infos);
	ParseDI(infos);
	ParseDO(infos);
}

// 状态端口消息读取
void RobotHandTest::OnStatusDataReady(const QByteArray& data)
{
	// 状态数据解析
	//log_error("{}", data.toHex());
	for (int i = 0; i * g_stateLength < data.size(); i++) {
		auto info = data.mid(i * g_stateLength, g_stateLength);
		if (info.size() != g_stateLength) {
			log_error("status size error ={}", info.toHex());
			continue;
		}
		if (info == m_lastData && m_stateTimes < 100) {	// 数据未改变且未超过最大此时，不需要解析数据
			m_stateTimes++;
			continue;
		}
		{
			QMutexLocker locker(&stateMutex);
			m_stateTimes = 0;
			m_lastData = info;
		}

		ParaseRobotStateData();	// 解析状态数据
	}
}
void RobotHandTest::OnDisConnection(int state)
{
	if (state == 0) {
		log_trace("状态端口连接成功");
		m_connect = 0;
	} else if (state == 1) {
		log_error("状态端口断开连接");
		m_connect = -1;
		emit ConnectionStateChanged(-1);
	} else if (state == 2) {
		log_error("命令端口断开连接");
		m_connect = -2;
	} else {
		log_error("state port {} is disconnected", state);
	}

}

// 状态端口消息读取

bool RobotHandTest::SendCmdData(int type, const QByteArray& data)
{
	QMutexLocker locker(&m_sendMutex);
	m_msgIndex++;

	QByteArray send = ToBin(g_dataLength, 4);	// 消息长度
	send.append(ToBin((int)m_msgIndex, 4)); // 消息编号
	send.append(ToBin(type, 4));	// 命令类型
	send.append(data);		// 命令数据
	while (send.size() < g_dataLength) {	// 消息填充
		send.append('\0');
	}
	log_trace("send to rob: {}", send.toHex());
	QByteArray ans;
	if (!m_cmd->SendMessageAndWaitAns(send, ans, 3000)) {
		log_error("send timeout 发送超时");
		return false;
	}
	int  ret = CheckCmdReturnVal(m_msgIndex, type, ans);
	if (ret == 0) {
		return true;
	} else {
		log_error("msg ans error");
		return false;
	}
}


/****************************************************************************
 * 功能: 校验命令返回值是否正确
 * 传参: 命令编号：index, 命令类型: type, 返回消息：ans
 * 返回值: 0: 消息校验正常， 1：还需要继续等待回复，-1：消息校验失败，无法收到回复
 ****************************************************************************/
int RobotHandTest::CheckCmdReturnVal(int index, int type, const QByteArray& ans)
{
	log_trace("check ans for type[{}], ans : {}", type, ans.toHex());
	if (ans.size() < g_dataLength) {	// 消息长度异常无法判断，继续等待
		log_error("ans size error");
		return 1;
	}

	int idx = FromBinToInt(ans.mid(0, 4));
	if (index != idx) {
		log_error("ans index: {}, cmd index:{}", idx, index);
		return -1;	// 消息编号不对应，继续判断下一条消息
	}
	int cmd = FromBinToInt(ans.mid(4, 4));
	if (cmd != type) {
		log_error("ans cmd type: {}, cmd type:{}", cmd, type);
		return -1;	// 编号对应，但是回复的内容不对，直接返回错误
	}
	return 0;
}
QString RobotHandTest::GetFaultMsg(int fault)
{
	switch (fault) {
	case 0:
		return {};
	case 11:
		return "通用错误：检查是否为手动模式";
	default:
		return "未知错误";
		break;
	}
	return {};
}


void RobotHandTest::ParseJoints(const QByteArrayList& d)
{
	Joints joint = {};
	joint.joint1 = d[0].toDouble();
	joint.joint2 = d[1].toDouble();
	joint.joint3 = d[2].toDouble();
	joint.joint4 = d[3].toDouble();
	joint.joint5 = d[4].toDouble();
	joint.joint6 = d[5].toDouble();
	emit RobotHandJoints(QString(joint.toByteArray()));
}
void RobotHandTest::ParseTargets(const QByteArrayList& d)
{
	PositionJ pos = {};
	pos.x = d.at(6).toDouble();
	pos.y = d.at(7).toDouble();
	pos.z = d.at(8).toDouble();
	pos.a = d.at(9).toDouble();
	pos.b = d.at(10).toDouble();
	pos.c = d.at(11).toDouble();

	emit RobotHandTargets(pos.toByteArray());
}
void RobotHandTest::ParsePointId(const QByteArrayList& d)
{
	int id = d.at(12).toInt();
	if (id > 0) {
		emit RobotHandPointId(id);
	}
}
void RobotHandTest::ParseMoving(const QByteArrayList& d)
{
	int move = d.at(13).toInt();
	emit RobotHandMoving(move);
}
void RobotHandTest::ParseToolId(const QByteArrayList& d)
{
	int tool = d.at(14).toInt();
	emit RobotHandToolID(tool);
}
void RobotHandTest::ParseFaultCode(const QByteArrayList& d)
{
	int fault = d.at(15).toInt();
	QString err;
	if (fault != 0) {
		err = GetFaultMsg(fault);
	}
	emit RobotHandFaultCode(fault, err);
}
void RobotHandTest::ParseDI(const QByteArrayList& d)
{
	QString di = d.at(16);
	emit RobotHandDIInfo(di);
}
void RobotHandTest::ParseDO(const QByteArrayList& d)
{
	QString dio = d.at(17);
	emit RobotHandDIInfo(dio);
}
